/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Heriot-Watt University, UK.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Heriot-Watt University nor the names of 
*     its contributors may be used to endorse or promote products 
*     derived from this software without specific prior written
*     permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Pedro Patron, Pierre-Yves Mignotte
*
*********************************************************************/

#ifndef _OSLCORE_MATRIX_H_
#define _OSLCORE_MATRIX_H_

#define TRUEE 1
#define FALSEE 0
#define MINI(x,y) ( (x<y) ? (x) : (y) )
#define MAXI(x,y) ( (x>y) ? (x) : (y) )

#include <assert.h>
#include <math.h>
#include <stdint.h>

#include <osl_core/Vector.h>

namespace osl_core {

template<class T>
class Matrix;

template<class S>
Matrix<S> operator +  (const Matrix<S> & A, const Matrix<S> & B);

template<class S>
Matrix<S> operator +  (const Matrix<S> & A, const S & k );

template<class S>
Matrix<S> operator -  (const Matrix<S> & A, const Matrix<S> & B);

template<class S>
Matrix<S> operator -  (const Matrix<S> & A);

template<class S>
Matrix<S> operator *  (const S & k, const Matrix<S> & A);

template<class S>
Matrix<S> operator *  (const Matrix<S> & A, const Matrix<S> & B);

template<class S>
Vector<S> operator *  (const Matrix<S> & A, const Vector<S> & v);

template<class S>
Matrix<S> operator *  (const Matrix<S> & A, const S & k);

template<class S>
Matrix<S> 			pow (const Matrix<S> & A,   uint32_t n);

template<class S>
Matrix<S>			trans (const Matrix<S> & A);

template<class S>
Matrix<S>			vector2matrixProduct ( const Vector<S> & a, const Vector<S> & b );

template<class S>
#if defined(WIN32) || defined(_WIN32)
std::istream & operator >> (std::istream & in, Matrix<S> & A);
#else
istream & operator >> (istream & in, Matrix<S> & A);
#endif

template<class S>
#if defined(WIN32) || defined(_WIN32)
std::ostream & operator << (std::ostream & out, const Matrix<S> & A);
#else
ostream & operator << (ostream & out, const Matrix<S> & A);
#endif

// Specific mathematical operations 

template<class S>
Matrix<double>	inv	(const Matrix<S> & A);

template<class S>
Matrix<double>	eye (const Matrix<S> & A);

template<class S>
Matrix<double>	eye (const uint32_t & size);


/**
 * Matrix data abstraction
 */
template <class T>
class Matrix {
	
	/**
	 * Matrix addition operation
	 */
	template<class S>
	friend Matrix<S> operator +	(const Matrix<S> & A, const Matrix<S> & B);

	/**
	 * Matrix + scalar operation
	 */
	template<class S>
	friend Matrix<S> operator +	(const Matrix<S> & A, const S & k );

	/**
	 * Matrix substraction operation
	 */
	template<class S>
	friend Matrix<S> operator -	(const Matrix<S> & A, const Matrix<S> & B);
	
	/**
	 * Matrix negation operation
	 */
	template<class S>
	friend Matrix<S> operator -	(const Matrix<S> & A);
	
	/**
	 * Value by Matrix product
	 */
	template<class S>
	friend Matrix<S> operator *	(const S & k, const Matrix<S> & A);

	/**
	 * Matrix by Matrix product
	 */
	template<class S>
	friend Matrix<S> operator *	(const Matrix<S> & A, const Matrix<S> & B);

	/**
	 * Matrix by Vector product
	 */	
	template<class S>
	friend Vector<S> operator *  (const Matrix<S> & A, const Vector<S> & v);

	/**
	 * Matrix by Value product
	 */
	template<class S>
	friend Matrix<S> operator *	(const Matrix<S> & A, const S & k);

	/**
	 * Matrix Power
	 */
	template<class S>
	friend Matrix<S> pow		(const Matrix<S> & A,   uint32_t n);
	
	/**
	 * Matrix Transpose
	 */
	template<class S>
	friend Matrix<S> trans		(const Matrix<S> & A);

	/**
	 * Matrix Inverse
	 */
	template<class S>
	friend Matrix<double> inv		(const Matrix<S> & A);

	/**
	 * Identity matrix of the same size as A
	 */
	template<class S>
	friend Matrix<double>	eye 	(const Matrix<S> & A);

	/**
	 * Identity matrix of size size
	 */
	template<class S>
	friend Matrix<double>	eye		(const uint32_t & size);
	
	/**
	 * Input matrix stream 
	 * @param	in	input stream
	 * @param	A	matrix
	 *
	 * Modifies the matrix to contain elements received through input stream
	 */
	template<class S>
	#if defined(WIN32) || defined(_WIN32)
	friend std::istream & operator >> (std::istream & in, Matrix<S> & A);
	#else
	friend istream & operator >> (istream & in, Matrix<S> & A);
	#endif

	/**
	 * Output matrix stream
	 * @param 	out	output stream
	 * @param	A	matrix
	 * @return	output stream describing matrix elements	
	 *
	 */
	template<class S>
	#if defined(WIN32) || defined(_WIN32)
	friend std::ostream & operator << (std::ostream & out, const Matrix<S> & A);
	#else
	friend ostream & operator << (ostream & out, const Matrix<S> & A);
	#endif

	public:
  
		/**
		 * Constructor with initial value
		 */
		#if defined(WIN32) || defined(_WIN32)
		Matrix	(const uint32_t & numberOfRows = 3,
						const uint32_t & numberOfColumns = 3,
					   	T initialValue = (T)0);
		#else
		Matrix	(const uint32_t & numberOfRows = 3,
						const uint32_t & numberOfColumns = 3,
					   	const T & initialValue = T());
		#endif

		/**
		 * Copy constructor
		 */
		Matrix	(const Matrix & source);

		/**
		 * Destructor
		 */
		virtual ~Matrix	();

		/**
		 * Element access
		 * @return index row
		 */
		Vector<T> &	operator []	(const uint32_t & index) const;
		
		/**
		 * @return index column
		 */
		Vector<T> 	getColumn	(const uint32_t & index) const;
		
		/**
		 * Asignation
		 */
		Matrix<T> &  	operator =	(const Matrix<T> & source);

		/**
		 * Get number of rows
		 */
		uint32_t	numberRows	() const;

		/**
		 * Get number of columns
		 */
		uint32_t	numberColumns	() const;

		/**
		 * Dynamic change of rows dimension
		 */
		uint32_t	setNumberRows		(const uint32_t & numberOfRows);
		
		/**
		 * Dynamic change of columns dimension
		 */
		uint32_t	setNumberColumns	(const uint32_t & numberOfColumns);
		
		/**
		 * Dynamic change of rows dimension with initial value
		 */
		uint32_t	setNumberRows		(const uint32_t & numberOfRows, const T & initialValue);

		/**
		 * Dynamic change of columns dimension with initial value
		 */
		unsigned int	setNumberColumns	(const unsigned int & numberOfColumns, const T & initialValue);
		
		/**
		 * Dynamic change of size with initial value
		 */
		void setSize( const uint32_t & numberOfRows, const uint32_t & numberOfColumns, const T & initialValue);

		/**
		 * Set all elements in matrix to value
		 */
		void	setInitialValue(const T & value);
	
		/**
		 * Set all elements in matrix to T() 0 but the diagonal to value
		 */
		void	setDiagonal	(const T & value);
		
		/**
		 * Set index column to the values stored in vector
		 */
		void	setColumn	(const unsigned int & index, Vector<T> & column);


		/**
		 * Set index column to the values stored in vector. If the index is bigger
		 *  than the current number of columns on the matrix, new columns are created 
		 *  with the initial value indicated.
		 */
		void	setColumn	(const unsigned int & index, Vector<T> & column, const T & initialValue );

		/**
		 * Set index row to the values stored in vector
		 */
		void	setRow		(const unsigned int & index, Vector<T> & row );

		/**
		 * Set index row to the values stored in vector. If the index is bigger
		 *  than the current number of columns on the matrix, new columns are created 
		 *  with the initial value indicated.
		 */
		void	setRow		(const unsigned int & index, Vector<T> & row, const T & initialValue);

	protected:
	
		/**
		 * Data area
		 */
		Vector<Vector<T>*>	rows;
};


//****************************************************************
template <class T>
#if defined(WIN32) || defined(_WIN32)
Matrix<T>::Matrix (const unsigned int & numberOfRows, const unsigned int & numberOfColumns, const T initialValue)
				: rows(numberOfRows)
#else
Matrix<T>::Matrix (const unsigned int & numberOfRows, const unsigned int & numberOfColumns, const T & initialValue)
				: rows(numberOfRows)
#endif
{
	// Creates and initializes a new matrix

	// Memory allocation for the row Vectors
	for (unsigned int i = 0; i < numberOfRows; i++) 
	{
		rows[i] = new Vector<T>(numberOfColumns, initialValue);
		assert(rows[i] != 0);
	}
}

//****************************************************************
template <class T>
Matrix<T>::Matrix (const Matrix & source)
				: rows(source.numberRows())
{
	// Copy constructor
	unsigned int numRows = rows.size();
	
	// Asignation for the row Vectors
	for (unsigned int i = 0; i < numRows; i++) 
	{
		if (rows[i] != NULL) 
				delete rows[i];
		rows[i] = new Vector<T>(*source.rows[i]);
		assert(rows[i] != 0);
	}
}

//****************************************************************
template <class T>
Matrix<T>::~Matrix ()
{
	// Free memory allocated dynamically
	unsigned int numRows = rows.size();
	
	for (unsigned int i = 0; i < numRows; i++) 
	{
		delete rows[i];
		rows[i] = 0;
	}
}

//****************************************************************
template <class T>
Matrix<T> & Matrix<T>::operator = (const Matrix<T> & source)
{
	assert (numberRows() == source.numberRows() && "Matrix::operator=() Different number of rows");
	assert (numberColumns() == source.numberColumns() && "Matrix::operator=() Different number of columns");

	// for all rows in number of rows
	unsigned int numRows = source.numberRows();
	for (unsigned int i = 0; i < numRows; i++) 
	{
		// clean old data
		delete rows[i];

		// asign new space for the elements
		rows[i] = new Vector<T>(*source.rows[i]);
		assert(rows[i] != 0);
	}
	
	return *this;
}

//****************************************************************
template <class T>
Vector<T> & Matrix<T>::operator [] (const unsigned int & index) const
{
	// Returns row vector of given index

	// check index validity
	assert(index < rows.size());
	
	// check reference
	assert(rows[index] != 0);
	
	return *rows[index];
}


//****************************************************************
template <class T>
Vector<T> Matrix<T>::getColumn (const unsigned int & index) const
{
	Vector<T>  v(rows.size());
	
	// check reference
	assert(rows[0] != 0);
	
	// check index validity
	assert(index < rows[0]->size());
	
	for (unsigned int i=0; i< rows.size(); i++)
	{
		v[i]=(*rows[i])[index];
	}

	return v;
}

//****************************************************************
template <class T>
unsigned int Matrix<T>::numberRows () const
{
	// Returns number rows of the matrix
	
	return rows.size();
}

//****************************************************************
template <class T>
unsigned int Matrix<T>::numberColumns () const
{
	// Returns number columns of the matrix
	
	// check that matrix contains rows
	assert(rows[0] != 0);
	
	return rows[0]->size();
}

//****************************************************************
template <class T>
unsigned int Matrix<T>::setNumberRows (const unsigned int & numberOfRows)
{
	unsigned int numColumns = numberColumns();
	unsigned int oldRows = rows.size();
	
	for (unsigned int i = numberOfRows; i < oldRows; i++) 
	{
		delete rows[i];
		rows[i] = 0;
	}
	
	rows.setSize(numberOfRows);
		
	for (unsigned int i = oldRows; i < numberOfRows; i++) 
	{
		rows[i] = new Vector<T>(numColumns);
		assert(rows[i] != 0);
	}
	
	return rows.size();
}

//****************************************************************
template <class T>
unsigned int Matrix<T>::setNumberRows (const unsigned int & numberOfRows, const T & initialValue)
{
	unsigned int numColumns = numberColumns();
	unsigned int oldRows = rows.size();
	
	for (unsigned int i = numberOfRows; i < oldRows; i++) 
	{
		delete rows[i];
	   	rows[i] = 0;
	}

	rows.setSize(numberOfRows);

	for (unsigned int i = oldRows; i < numberOfRows; i++) 
	{
		rows[i] = new Vector<T>(numColumns, initialValue);
		assert(rows[i] != 0);
	}
	
	return rows.size();
}

//****************************************************************
template <class T>
unsigned int Matrix<T>::setNumberColumns (const unsigned int & numberOfColumns)
{
	unsigned int numRows = rows.size();

	for (unsigned int i = 0; i < numRows; i++)
	{
		rows[i]->setSize(numberOfColumns);
	}

	return numberOfColumns;
}


//****************************************************************
template <class T>
unsigned int Matrix<T>::setNumberColumns (const unsigned int & numberOfColumns, const T & initialValue)
{
	unsigned int numRows = rows.size();
	
	for (unsigned int i = 0; i < numRows; i++)
	{
		rows[i]->setSize(numberOfColumns, initialValue);
	}

	return numberOfColumns;
}

//****************************************************************
template <class T>
void Matrix<T>::setSize( const uint32_t & numberOfRows, const uint32_t & numberOfColumns, const T & initialValue)
{
	setNumberRows( numberOfRows, initialValue );

	setNumberColumns( numberOfColumns, initialValue );
}

//****************************************************************
template <class T>
void	Matrix<T>::setInitialValue(const T & value)
{

	for (unsigned int i=0; i < numberRows(); i++)
	{
		for (unsigned int j=0; j< numberColumns(); j++)
		{
			(*this)[i][j]=value;
		}
	}
}
	
//****************************************************************
template <class T>
void	Matrix<T>::setDiagonal	(const T & value)
{
	assert( (numberRows() == numberColumns()) && "Matrix::setDiagonal() #Rows != #Columns");
		
	for (unsigned int i=0; i < numberRows(); i++)
	{
		(*this)[i][i]=value;
	}
}		

//****************************************************************
template <class T>
void	Matrix<T>::setColumn	(const unsigned int & index, Vector<T> & column)
{
	assert (index < numberColumns());
	for (unsigned int i=0; i < numberRows(); i++)
	{
		(*this)[i][index]=column[i];
	}
}
//****************************************************************
template <class T>
void	Matrix<T>::setColumn	(const unsigned int & index, Vector<T> & column, const T & initialValue )
{
	if( index + 1 > numberColumns() )
		setNumberColumns( index+1, initialValue );

	for (unsigned int i=0; i < numberRows(); i++)
	{
		(*this)[i][index]=column[i];
	}
}
//****************************************************************
template <class T>
void	Matrix<T>::setRow	(const unsigned int & index, Vector<T> & row)
{
	assert (index < numberRows());

	assert ( row.length() == numberColumns() );

	(*this)[index] = row;

}
//****************************************************************
template <class T>
void	Matrix<T>::setRow	(const unsigned int & index, Vector<T> & row, const T & initialValue)
{
	if( index + 1 > numberRows() )
		setNumberRows( index+1, initialValue );

	assert ( row.length() == numberColumns() );

	(*this)[index] = row;

}

// Mathematical operators

//****************************************************************
template <class T>
Matrix<T> operator + (const Matrix<T> & A, const Matrix<T> & B)
{
	assert(A.numberRows() == B.numberRows() && A.numberColumns() == B.numberColumns());
	
	Matrix<T> S = A;
	
	for (unsigned int i = 0; i < S.numberRows(); i++) 
	{
		for (unsigned int j=0; j< S.numberColumns(); j++)
		{
			S[i][j] += B[i][j];
		}
	}
	
	return S;
}
//****************************************************************
template <class T>
Matrix<T> operator + (const Matrix<T> & A, const T & k )
{
	// Adds all elements the constant K
	Matrix<T> S(A.numberRows(),A.numberColumns());
	
	for (unsigned int i = 0; i < A.numberRows(); i++) 
	{
		for (unsigned int j = 0; j < A.numberColumns(); j++)
		{
			S[i][j] = k+A[i][j];
		}
	}
  
	return S;
}

//****************************************************************
template <class T>
Matrix<T> operator - (const Matrix<T> & A, const Matrix<T> & B)
{
	// Calculates matrix substraction

	assert(A.numberColumns() == B.numberColumns() && A.numberRows() == B.numberRows());
	
	return A + (-B);
}

//****************************************************************
template <class T>
Matrix<T> operator - (const Matrix<T> & A)
{
	// Changes sign of all elements

	return  ((T) -1)*A;
}

//****************************************************************
template <class T>
Matrix<T> operator * (const T & k, const Matrix<T> & A)
{
	// Multiplies all elements by the constant K
	Matrix<T> S(A.numberRows(),A.numberColumns());
	
	for (unsigned int i = 0; i < A.numberRows(); i++) 
	{
		for (unsigned int j = 0; j < A.numberColumns(); j++)
		{
			S[i][j] = k*A[i][j];
		}
	}
  
	return S;
}

//****************************************************************
template <class T>
Matrix<T> operator * (const Matrix<T> & A, const T & k)
{
	return k*A;
}


//****************************************************************
template <class T>
Vector<T> operator * (const Matrix<T> & A, const Vector<T> & v)
{
	assert (A.numberColumns()==v.size());

	Vector<T> s(A.numberRows());
				
	for (unsigned int i = 0; i < A.numberRows(); i++)
	{
		T elem = (T) 0;
		for (unsigned int j = 0; j < A.numberColumns(); j++) 
		{
			elem += A[i][j]*v[j];
		}
		s[i]=elem;
	}
		
	return s;
}

//****************************************************************
template <class T>
Matrix<T> operator * (const Matrix<T> & A, const Matrix<T> & B)
{
	assert (A.numberColumns()==B.numberRows());
	
	Matrix<T> S(A.numberRows(),B.numberColumns());
	
	for (unsigned int i = 0; i < A.numberRows(); i++) 
	{
		for (unsigned int j = 0; j < B.numberColumns(); j++) 
		{
			T elem = (T) 0;
			for (unsigned int k = 0; k < A.numberColumns(); k++)
			{
				elem += A[i][k]*B[k][j];
			}
			S[i][j]=elem;
		}
	}
	
	return S;
}

//****************************************************************
template <class T>
Matrix<T> pow (const Matrix<T> & A, unsigned int n)
{
	Matrix<T> S=A;
	
	for (unsigned int i=1; i<n; i++)
	{
		S = S*A;
	}
	
	return S;
}

//****************************************************************
template <class T>
Matrix<T> trans (const Matrix<T> & A)
{
	Matrix<T> S(A.numberColumns(),A.numberRows());
	
	for (unsigned int i = 0; i < A.numberColumns(); i++)
	{
		for (unsigned int j = 0; j < A.numberRows(); j++)
		{
			S[i][j]=A[j][i];
		}
	}

	return S;
}


//******************************************************************
template <class T>
Matrix<T> trans (const Vector<T> & A)
{
	Matrix<T> S(1, A.length());
	
		for (unsigned int j = 0; j < A.length(); j++)
		{
			S[0][j]=A[j];
		}

	return S;
}


/*
 * This will estimate the determinant of the matrix based on the LU decomposition.
 * @return Determinant of the matrix
 */
 
 // History:
 // 20/03/07: Pierre-Yves Mignotte, pierre-yves.mignotte@seebyte.com - Original code
 
template <class T>
double det (const Matrix<T> & A)
{
	assert (A.numberColumns() == A.numberRows());
	
	Matrix<double> L (A.numberColumns(), A.numberRows(), 0.0);
	Matrix<double> U (A.numberColumns(), A.numberRows(), 0.0);
	
	double det = LU(A,&L,&U);
	
	return det;
}

/*
 * This will perform LU decomp on matrix A return matrix L, matrix U
 * lu(A(n,n)) => L(n,n) and U(n,n)
 * @return Determinant of the matrix
 * Unsafe to call with A == L or A == U
 */
template <class T>
double LU (const Matrix<T> & A, Matrix<double> *L, Matrix<double> *U)
{
	assert (A.numberColumns()  == A.numberRows());
	assert (L->numberColumns() == L->numberRows());
	assert (U->numberColumns() == U->numberRows());
	assert (A.numberColumns()  == L->numberColumns());
	assert (A.numberColumns()  == U->numberColumns());
	
	uint32_t	n = A.numberRows();
	
	Matrix<double> A_c (A.numberColumns(), A.numberRows(), 0.0);
	
	// Copy A matrix
	for (uint32_t i = 0; i < n; i++)
	{
		for (uint32_t j = 0; j < n; j++)
		{
			A_c[i][j] = (double) A[i][j];
		}
	}
		

	for (uint32_t k=0; k < n-1; k++)
	{
		for (uint32_t i = k+1; i<n; i++)
		{
			A_c[i][k] = (double) A_c[i][k] / A_c[k][k];
			for (uint32_t j=k+1; j < n; j++)
			{
				A_c[i][j] -= (double) A_c[i][k] * A_c[k][j];
			}
		}
	}

	// Separate L
	L->setInitialValue(0);
	L->setDiagonal(1);
	
	for (uint32_t j=0; j<n-1; j++)
	{
		for (uint32_t i=j+1; i<n; i++)
		{
			(*L)[i][j] = A_c[i][j];
		}
	}

	// Separate U
	U->setInitialValue(0);
	for (uint32_t i=0; i<n; i++)
	{
		for (uint32_t j=i; j<n; j++)
		{
			(*U)[i][j] = A_c[i][j];
		}
	}

	// The determinant is computed as the product of all the diagonal elements
	// Linear algebra: det(A)=det(LU)=det(L) * det(U)
	double det = 1;
	for (uint32_t i=0; i<n; i++) {
		det *= ((*U)[i][i]*(*L)[i][i]);
	}
	return det;

}

/*
 *This will solve A*x = b, where double* A is upper triangular
 *A(n,n)*x(n,1) = b(n,1)
 */
template<class T>
Vector<double> solveUpperTriangular(const Matrix<T> & A, const Vector<double> & b)
{
	assert (A.numberColumns() == A.numberRows());
	assert (A.numberColumns() == b.size());

	double	sum = 0.0;
	uint32_t		n = A.numberColumns();
	Vector<double>  x(A.numberColumns(),0);
	Matrix<double> A_c (A.numberColumns(), A.numberRows(), 0.0);
	
	
	// Copy A matrix
	for (uint32_t i = 0; i < n; i++)
	{
		for (uint32_t j = 0; j < n; j++)
		{
			A_c[i][j] = (double) A[i][j];
		}
	}
	
	
	for (int32_t i = (n-1); i >= 0; i--)
	{
		sum = b[i];
		
		for (uint32_t j = i+1; j < n; j++)
		{	
			sum -= A_c[i][j]*x[j];
		}

		x[i] = sum/A_c[i][i];
	}

	return x;

}



/*
 * This will solve A*x = b, where double* A is lower triangular
 * A(n,n)*x(n,1) = b(n,1)
 */
template<class T>
Vector<double> solveLowerTriangular (const Matrix<T> & A, const Vector<double> & b)
{
	assert (A.numberColumns() == A.numberRows());
	assert (A.numberColumns() == b.size());
	
	double	sum = 0.0;
	uint32_t		n = A.numberRows();
	Vector<double>	x(A.numberColumns(),0.0);
	Matrix<double> A_c (A.numberColumns(), A.numberRows(), 0.0);
	
	// Copy A matrix
	for (uint32_t i = 0; i < n; i++)
	{
		for (uint32_t j = 0; j < n; j++)
		{
			A_c[i][j] = (double) A[i][j];
		}
	}
	
	

	for (uint32_t i=0; i < n; i++)
	{
		sum = b[i];

		for (uint32_t j = 0; j < i; j++)
		{
			sum -= A_c[i][j]*x[j];
		}

		x[i] = sum/A_c[i][i];
	}

	return x;
}

//****************************************************************
template<class T>
Matrix<double> inv		(const Matrix<T> & A)
{
	assert (A.numberColumns() == A.numberRows());
	
	Matrix<double> S (A.numberColumns(), A.numberRows(), 0.0);
	Matrix<double> L (A.numberColumns(), A.numberRows(), 0.0);
	Matrix<double> U (A.numberColumns(), A.numberRows(), 0.0);
	
	Matrix<double> invL (A.numberColumns(), A.numberRows(), 0.0);
	Matrix<double> invU (A.numberColumns(), A.numberRows(), 0.0);
	
	Vector<double> iColumn (A.numberColumns(), 0.0);
	Vector<double> invLCol (A.numberColumns(), 0.0);
	Vector<double> invUCol (A.numberColumns(), 0.0);
	
	// construct an identity matrix
	S.setDiagonal(1);
	
	// perform LU decomp on A 
	double det = LU(A,&L,&U);
	
	// check if this determinant is acceptable
	if (det!=0) {
		for (uint32_t i = 0; i < A.numberRows(); i++)
		{
			// separates the ith column
			iColumn = S.getColumn(i);

			invUCol = solveUpperTriangular(U, iColumn);

			invLCol = solveLowerTriangular(L, iColumn);
		
			// places invUcol in ith column of invU
			invU.setColumn(i, invUCol);

			// places invLcol in ith column of invL
			invL.setColumn(i, invLCol);
		}
	
		// inv(A) = inv(U)*inv(L) 
		S = (invU) * (invL);
	}

	return S;
}
		
//****************************************************************
template <class T>
Matrix<double>	eye 	(const Matrix<T> & A)
{
	assert(A.numberRows() == A.numberColumns());

	Matrix<double> matrix(A.numberRows(),A.numberColumns(),0);

	matrix.setDiagonal(1);
	
	return matrix;
}

//****************************************************************
template <class T>
Matrix<double>	eye		(const uint32_t & size)
{
	Matrix<double> matrix(size,size,0);

	matrix.setDiagonal(1);
	
	return matrix;
}

//****************************************************************
template <class T>
#if defined(WIN32) || defined(_WIN32)
std::ostream & operator << (std::ostream & out, const Matrix<T> & A)
#else
ostream & operator << (ostream & out, const Matrix<T> & A)
#endif
{
	for (unsigned int i = 0; i < A.numberRows(); i++)
	{
		out << A[i] << std::endl;
	}
	
	return out;
}

//****************************************************************
template <class T>
#if defined(WIN32) || defined(_WIN32)
std::istream & operator >> (std::istream & in, Matrix<T> & A)
#else
istream & operator >> (istream & in, Matrix<T> & A)
#endif
{
	for (unsigned int i = 0; i < A.numberRows(); i++) 
	{
		in >> A[i];
		assert(A[i].size() == A.numberColumns());
	}
	
	return in;
}



typedef struct svdresult
{
	//     A    =    U    .     S     .   Vt
	// (mxn) = (mxn) . (nxn) . (nxn)
	Matrix<double> * U;
	Matrix<double> * S;
	Matrix<double> * V;
} SVDResult;


// ****************************************************************************
/*
 * Compute sqrt(a2+b2) without numerical issues...
 */

template <class T>
T hypot (T a, T b) 
{
	T r;
    if (fabs(a) > fabs(b)) {
        r = b/a;
        r = fabs(a)*sqrt(1+r*r);
    } else if (b != 0) {
        r = a/b;
        r = fabs(b)*sqrt(1+r*r);
    } else {
        r = 0.0;
    }
    return r;
}

// ****************************************************************************
/*
 * This will perform a singular value decomposition. This code is based on JAMA
 * library (Java Matrices) . A = USVt. Give the same results than [u,s,v] = svd(A, 'econ');
 * under matlab.
 */
template <class T>
SVDResult SVD (const Matrix<T>  A)
{
	int32_t iLoop, jLoop, k; // anonymous looping variable

    int32_t pp = 0;
    int32_t iter = 0;
    const double eps = ::pow(2.0,-52.0);
    const double tiny = ::pow(2.0,-966.0);
	int32_t kase = 0, ks = 0;
	double f=0, g=0, cs=0, sn=0, scale=0, sp=0, spm1=0, epm1=0, sk=0, ek=0, b=0, c=0, shift=0;


	int32_t m = A.numberRows();
	int32_t n = A.numberColumns();

	// nu is the smallest dimension. This algo should work with it. Otherwise... hmm, let m>=n
	int32_t nu = n;
	if (nu>m) {
		nu = m;
	}
	
	// Initialisation of the structure holding the output.
	SVDResult out;
	
	if (m+1 <= n) {
		out.S = new Matrix<double>(m+1,m+1,0.0);
	} else {
		out.S = new Matrix<double>(n,n,0.0);
	}
	out.U = new Matrix<double>(m,nu,0.0);
	out.V = new Matrix<double>(n,n,0.0);

	// Initialisation of variables.
	double * e = new double[n];
	for (iLoop=0; iLoop<n ;iLoop++) {
		e[iLoop] = 0;
	}
	double * work = new double[m];
	for (iLoop=0;iLoop<m;iLoop++) {
		work[iLoop] = 0;
	}

    uint16_t wantu = TRUEE;
    uint16_t wantv = TRUEE;
	
    // Reduce A to bidiagonal form, storing the diagonal elements
    // in s and the super-diagonal elements in e.
    int32_t nct = MINI(m-1,n);
    int32_t nrt = MINI(n-2,m);
	if (nrt<0) {
		nrt = 0;
	}
	
	double t; // local variable
	
    for (k = 0; k < MAXI(nct,nrt) ; k++) {

        if (k < nct) {
            // Compute the transformation for the k-th column and
            // place the k-th diagonal in s[k].
            // Compute 2-norm of k-th column without under/overflow.

            (*(out.S))[k][k] = 0;
            for (iLoop = k; iLoop < m; iLoop++) {
               (*(out.S))[k][k] = hypot((*(out.S))[k][k],A[iLoop][k]);
            }
			
            if ((*(out.S))[k][k] != 0.0) {
				if (A[k][k] < 0.0) {
					(*(out.S))[k][k] *= -1.0;
                }
				for (iLoop = k; iLoop < m; iLoop++) {
					A[iLoop][k] /= (T)(*(out.S))[k][k];
                }
                A[k][k] += 1.0;
            }
            (*(out.S))[k][k] *= -1.0;
        }
		
        for (jLoop = k+1; jLoop < n; jLoop++) {

			if ( (k < nct) & ((*(out.S))[k][k] != 0.0) )  {

				// Apply the transformation.
				t = 0.0;
                for (iLoop = k; iLoop < m; iLoop++) {
					t += A[iLoop][k]*A[iLoop][jLoop];
                }
				
                t /= (-A[k][k]);
                for (iLoop = k; iLoop < m; iLoop++) {
					A[iLoop][jLoop] += (T)(t*A[iLoop][k]);
                }
            }

            // Place the k-th row of A into e for the
            // subsequent calculation of the row transformation.
            e[jLoop] = A[k][jLoop];
        }
	
        if ( (wantu == TRUEE) & (k < nct) ) {

			// Place the transformation in U for subsequent back
            // multiplication.
            for (iLoop = k; iLoop < m; iLoop++) {
				(*(out.U))[iLoop][k] = A[iLoop][k];
            }
        }
		
        if ( k < nrt ) {
            // Compute the k-th row transformation and place the
            // k-th super-diagonal in e[k].
            // Compute 2-norm without under/overflow.

            e[k] = 0.0;
            for (iLoop = k+1; iLoop < n; iLoop++) {
				e[k] = hypot(e[k], e[iLoop]);
            }
			
            if ( e[k] != 0.0 ) {
                if (e[k+1] < 0.0) {
					e[k] *= -1.0;
                }
                for (iLoop = k+1; iLoop < n; iLoop++) {
					e[iLoop] /= e[k];
                }
                e[k+1] += 1.0;
            }
            e[k] *= -1.0;
			
            if ( (k+1 < m) & (e[k] != 0.0) ) {
				// Apply the transformation.
                for (iLoop = k+1; iLoop < m; iLoop++) {
					work[iLoop] = 0.0;
                }
				
                for (jLoop = k+1; jLoop < n; jLoop++) {
                    for (iLoop = k+1; iLoop < m; iLoop++) {
						work[iLoop] += e[jLoop]*A[iLoop][jLoop];
                    }
                }
				
                for (jLoop = k+1; jLoop < n; jLoop++) {
					t = -e[jLoop]/e[k+1];
					for (iLoop = k+1; iLoop < m; iLoop++) {
						A[iLoop][jLoop] += (T)(t*work[iLoop]);
					}
				}
            }
			
            if (wantv == TRUEE) {
				// Place the transformation in V for subsequent
				// back multiplication.
				for (iLoop = k+1; iLoop < n; iLoop++) {
					(*(out.V))[iLoop][k] = e[iLoop];
                }
            }
        }
    }

	// Set up the final bidiagonal matrix or order p.

	int32_t p = MINI(n,m+1);
    if (nct < n) {
        (*(out.S))[nct][nct] = A[nct][nct];
    }
	if (m < p) {
        (*(out.S))[p-1][p-1] = 0.0;
    }
    if (nrt+1 < p) {
        e[nrt] = A[nrt][p-1];
    }
    e[p-1] = 0.0;

    // If required, generate U.
    if ( wantu == TRUEE ) {
	
        for (jLoop = nct; jLoop < nu; jLoop++) {
            for (iLoop = 0; iLoop < m; iLoop++) {
               (*(out.U))[iLoop][jLoop] = 0.0;
            }
            (*(out.U))[jLoop][jLoop] = 1.0;
        }
    
		for (k = nct-1; k >= 0; k--) {

            if ( (*(out.S))[k][k] != 0.0) {
                for (jLoop = k+1; jLoop < nu; jLoop++) {
					t = 0.0;
					for (iLoop = k; iLoop < m; iLoop++) {
						t += (*(out.U))[iLoop][k] * (*(out.U))[iLoop][jLoop];
					}
                    t /= (-(*(out.U))[k][k]);
                    for (iLoop = k; iLoop < m; iLoop++) {
						(*(out.U))[iLoop][jLoop] += t * (*(out.U))[iLoop][k];
                    }
                }
                for (iLoop = k; iLoop < m; iLoop++ ) {
					(*(out.U))[iLoop][k] *= - 1.0;
                }
				(*(out.U))[k][k] += 1.0;
                for (iLoop = 0; iLoop < (int)k-1; iLoop++) {
					(*(out.U))[iLoop][k] = 0.0;
                }
				
            } else {
			
                for (iLoop = 0; iLoop < m; iLoop++) {
					(*(out.U))[iLoop][k] = 0.0;
                }
                (*(out.U))[k][k] = 1.0;
				
            }
        }
    }

    // If required, generate V.
    if ( wantv == TRUEE ) {
        for (k = n-1; k >= 0; k--) {
            if ( (k < nrt) && (e[k] != 0.0)) {
                for (jLoop = k+1; jLoop < nu; jLoop++) {
                    t = 0.0;
                    for (iLoop = k+1; iLoop < n; iLoop++) {
						t += (*(out.V))[iLoop][k] * (*(out.V))[iLoop][jLoop];
                    }
					t /= (-(*(out.V))[k+1][k]);
					for (iLoop = k+1; iLoop < n; iLoop++) {
						(*(out.V))[iLoop][jLoop] += t*(*(out.V))[iLoop][k];
					}
				}
            }
            for (iLoop = 0; iLoop < n; iLoop++) {
				(*(out.V))[iLoop][k] = 0.0;
            }
            (*(out.V))[k][k] = 1.0;
        }
    }

	// Main iteration loop for the singular values.


	pp = p-1;
	iter = 0;
    while ((p > 0) && (iter < 1000)) {
        // Here is where a test for too many iterations would go.
		iter++;

        // This section of the program inspects for
        // negligible elements in the s and e arrays.  On
        // completion the variables kase and k are set as follows.

		// kase = 1     if s(p) and e[k-1] are negligible and k<p
        // kase = 2     if s(k) is negligible and k<p
        // kase = 3     if e[k-1] is negligible, k<p, and
        //              s(k), ..., s(p) are not negligible (qr step).
        // kase = 4     if e(p-1) is negligible (convergence).

        for (k = p-2; k >= -1; k--) {
            if (k == -1) {
				break;
            }
            if (fabs(e[k]) <= tiny + eps*( fabs((*(out.S))[k][k]) + fabs((*(out.S))[k+1][k+1]) ) ) {
				e[k] = 0.0;
				break;
            }
        }
        if (k == p-2) {
            kase = 4;
        } else {
            for (ks = p-1; ks >= k; ks--) {
                if (ks == k) {
					break;
                }
				
                t = (ks != p ? fabs(e[ks]) : 0.0) + (ks != k+1 ? fabs(e[ks-1]) : 0.0);
                
				if ( fabs((*(out.S))[ks][ks]) <= tiny + eps*t )  {
					(*(out.S))[ks][ks] = 0.0;
					break;
				}
            }
            if (ks == k) {
				kase = 3;
            } else if (ks == p-1) {
				kase = 1;
            } else {
				kase = 2;
				k = ks;
            }
        }
        k++;

        // Perform the task indicated by kase.

        switch (kase) {

            // Deflate negligible s(p).
            case 1:
                f = e[p-2];
                e[p-2] = 0.0;
				for (jLoop = p-2; jLoop >= k; jLoop--) {
					t = hypot((*(out.S))[jLoop][jLoop], f);
					cs = (*(out.S))[jLoop][jLoop]/t;
					sn = f/t;
					(*(out.S))[jLoop][jLoop] = t;
					if (jLoop != k) {
						f = -sn*e[jLoop-1];
						e[jLoop-1] *= cs;
					}
					if ( wantv == TRUEE ) {
						for (iLoop = 0; iLoop < n; iLoop++) {
							t = cs*(*(out.V))[iLoop][jLoop] + sn*(*(out.V))[iLoop][p-1];
							(*(out.V))[iLoop][p-1] = -sn * (*(out.V))[iLoop][jLoop] + cs * (*(out.V))[iLoop][p-1];
							(*(out.V))[iLoop][jLoop] = t;
						}
					}
				}
                break;

            // Split at negligible s(k).
            case 2: 
				f = e[k-1];
				e[k-1] = 0.0;
				for (jLoop = k; jLoop < p; jLoop++) {
					t = hypot((*(out.S))[jLoop][jLoop],f);
					cs = (*(out.S))[jLoop][jLoop]/t;
					sn = f/t;
					(*(out.S))[jLoop][jLoop] = t;
					f = -sn*e[jLoop];
					e[jLoop] *= cs;
					if (wantu == TRUEE) {
						for (iLoop = 0; iLoop < m; iLoop++) {
							t = cs*(*(out.U))[iLoop][jLoop] + sn*(*(out.U))[iLoop][k-1];
							(*(out.U))[iLoop][k-1] = -sn*(*(out.U))[iLoop][jLoop] + cs*(*(out.U))[iLoop][k-1];
							(*(out.U))[iLoop][jLoop] = t;
						}
					}
				}
                break;

            // Perform one qr step.
            case 3:

				// Calculate the shift.
                scale = MAXI( fabs((*(out.S))[p-1][p-1]), fabs((*(out.S))[p-2][p-2]) );
				scale = MAXI( scale, fabs(e[p-2]) );
				scale = MAXI( scale, fabs(e[k]) );
				scale = MAXI( scale, fabs((*(out.S))[k][k]) );
				sp = (*(out.S))[p-1][p-1]/scale;
				spm1 = (*(out.S))[p-2][p-2]/scale;
				epm1 = e[p-2]/scale;
				sk = (*(out.S))[k][k]/scale;
				ek = e[k]/scale;
				b = ((spm1 + sp)*(spm1 - sp) + epm1*epm1)/2.0;
				c = (sp*epm1)*(sp*epm1);
				shift = 0.0;
				if ((b != 0.0) | (c != 0.0)) {
					shift = sqrt(b*b + c);
					if (b < 0.0) {
						shift *= -1.0;
					}
					shift = c/(b + shift);
				}
               
				f = (sk + sp)*(sk - sp) + shift;
				g = sk*ek;
   
				// Chase zeros.
   				for (jLoop = k; jLoop < p-1; jLoop++) {
					t = hypot(f,g);
					cs = f/t;
					sn = g/t;
					if (jLoop != k) {
						e[jLoop-1] = t;
					}
					f = cs*((*(out.S))[jLoop][jLoop]) + sn*e[jLoop];
					e[jLoop] = cs*e[jLoop] - sn* (*(out.S))[jLoop][jLoop];
					g = sn*(*(out.S))[jLoop+1][jLoop+1];
					(*(out.S))[jLoop+1][jLoop+1] *= cs;
										
					if ( wantv == TRUEE ) {
						for (iLoop = 0; iLoop < n; iLoop++) {
							t = cs * (*(out.V))[iLoop][jLoop] + sn * (*(out.V))[iLoop][jLoop+1];
							(*(out.V))[iLoop][jLoop+1] = -sn* (*(out.V))[iLoop][jLoop] + cs*(*(out.V))[iLoop][jLoop+1];
							(*(out.V))[iLoop][jLoop] = t;
						}
					}
					
					t = hypot(f,g);
					cs = f/t;
					sn = g/t;
					((*(out.S))[jLoop][jLoop]) = t;
					
					f = cs*e[jLoop] + sn*(*(out.S))[jLoop+1][jLoop+1];
					(*(out.S))[jLoop+1][jLoop+1] = -sn*e[jLoop] + cs*(*(out.S))[jLoop+1][jLoop+1];
					g = sn*e[jLoop+1];
					e[jLoop+1] = cs*e[jLoop+1];
					if ( (wantu == TRUEE) && (jLoop < m-1)) {
						for (iLoop = 0; iLoop < m; iLoop++) {
							t = cs*(*(out.U))[iLoop][jLoop] + sn*(*(out.U))[iLoop][jLoop+1];
							(*(out.U))[iLoop][jLoop+1] = -sn* (*(out.U))[iLoop][jLoop] + cs*(*(out.U))[iLoop][jLoop+1];
							(*(out.U))[iLoop][jLoop] = t;
						}
					}
				}
				e[p-2] = f;
                break;

            // Convergence.

            case 4:

				// Make the singular values positive.
                if ( (*(out.S))[k][k] <= 0.0) {
					(*(out.S))[k][k] *= -1.0;//((*(out.S))[k][k] < 0.0 ? -s[k] : 0.0);
					if (wantv == TRUEE) {
						for (iLoop = 0; iLoop <= pp; iLoop++) {
							(*(out.V))[iLoop][k] *= -1.0;
						}
					}
				}
   
				// Order the singular values.
   				while (k < pp) {
					if ( ( *(out.S))[k][k] >= ( *(out.S))[k+1][k+1]) {
						break;
					}
					t = (*(out.S))[k][k];
					(*(out.S))[k][k] = (*(out.S))[k+1][k+1];
					(*(out.S))[k+1][k+1] = t;
					
					if ( (wantv==TRUEE) && (k < n-1)) {
						for (iLoop = 0; iLoop < n; iLoop++) {
							t = (*(out.V))[iLoop][k+1]; 
							(*(out.V))[iLoop][k+1] = (*(out.V))[iLoop][k]; 
							(*(out.V))[iLoop][k] = t;
						}
					}
					
					if ( (wantu==TRUEE) && (k < m-1)) {
						for (iLoop = 0; iLoop < m; iLoop++) {
							t = (*(out.U))[iLoop][k+1]; 
							(*(out.U))[iLoop][k+1] = (*(out.U))[iLoop][k]; 
							(*(out.U))[iLoop][k] = t;
						}
					}
					k++;
				}
				p--;
				break;
        }
    }
	
	delete[] e;
	delete[] work;

    // If the iteration does not converge
	if (iter > 999) {
		delete out.S;
		out.S = NULL;
		delete out.U;
		out.U = NULL;
		delete out.V;
		out.V = NULL;
	}
	
	return out;
}


/**
* vector2matrixProduct
* @version 0.1
* @author Pierre-Yves Mignotte, pierre-yves.mignotte@seebyte.com
* @param a Left hand side vector (vertical)
* @param b Right hand side vector (vertical)
* @return A matrix equal to a*trans(b) #
*
* This function can used especially for the estimation of covariance matrices.
*/

template<class S>
Matrix<S>			vector2matrixProduct ( const Vector<S> & a, const Vector<S> & b )
{
	Matrix<S> finalProduct( a.size(), b.size(), (S)0 );
	
	for (uint32_t i=0; i<a.size(); ++i) {
		for (uint32_t j=0; j<b.size(); ++j) {
			finalProduct[i][j] = ( (a[i]) * (b[j]) );
		}
	}

	return finalProduct;
}

} // namespace

#endif
